using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Trees;
using BepuUtilities.Memory;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics
{
    public interface IShapeRayHitHandler
    {
        /// <summary>
        /// 检查是否应针对光线测试可碰撞对象的子级。仅由可以有多个子级的形状类型调用。
        /// </summary>
        /// <param name="Child Index">父对象中候选对象的索引。</param>
        /// <return>如果应该通过射线测试子对象,则为True,否则为False。</return>
        bool AllowTest(int childIndex);
        /// <summary>
        /// 在发现射线撞击时调用。
        /// </summary>
        /// <param name="ray">有关与此命中关联的射线的信息。</param>
        /// <param name="maxumT">允许沿射线行进的最大距离,以射线方向长度为单位。可以设置为限制将来的测试。</param>
        /// <param name="t">沿光线到影响的距离,以光线方向长度为单位。换句话说,hitLocation=ray.Origin+ray.Direction*t.</param>
        /// <param name="Normal">命中位置的表面法线。</param>
        /// <param name="Child Index">命中的子项的索引。对于没有多个子项的凸形或其他类型,该值始终为零。</param>
        void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex);
    }

    /// <summary>
    /// 定义能够过滤光线测试候选项和处理光线命中结果的类型。
    /// </summary>
    public interface IRayHitHandler
    {
        /// <summary>
        /// 检查是否应针对光线测试由加速度结构标识的可碰撞对象。
        /// </summary>
        /// <param name="Collisible">光线测试的可碰撞候选对象。</param>
        /// <return>如果应通过射线测试可碰撞对象,则为True,否则为False。</return>
        bool AllowTest(CollidableReference collidable);
        /// <summary>
        /// 检查是否应针对光线测试可碰撞对象的子级。仅由可以有多个子级的形状类型调用。
        /// </summary>
        /// 候选人的<param name="Collisible">父级。</param>
        /// <param name="Child Index">父对象中候选对象的索引。</param>
        /// <return>如果应该通过射线测试子对象,则为True,否则为False。</return>
        bool AllowTest(CollidableReference collidable, int childIndex);
        /// <summary>
        /// 在发现射线撞击时调用。
        /// </summary>
        /// <param name="ray">有关与此命中关联的射线的信息。</param>
        /// <param name="maxumT">允许沿射线行进的最大距离,以射线方向长度为单位。可以设置为限制将来的测试。</param>
        /// <param name="t">沿光线到影响的距离,以光线方向长度为单位。换句话说,hitLocation=ray.Origin+ray.Direction*t.</param>
        /// <param name="Normal">命中位置的表面法线。</param>
        /// <param name="Collisible">射线击中的可碰撞。</param>
        /// <param name="Child Index">命中的子项的索引。对于没有多个子项的凸形或其他类型,该值始终为零。</param>
        void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, CollidableReference collidable, int childIndex);
    }

    /// <summary>
    /// 定义能够过滤候选扫描和处理扫描结果的类型。
    /// </summary>
    public interface ISweepHitHandler
    {
        /// <summary>
        /// 检查是否对可碰撞目标运行详细扫描测试。
        /// </summary>
        /// <param name="Collisible">可碰撞检查。</param>
        /// <return>如果应尝试扫描测试,则为True,否则为False。</return>
        bool AllowTest(CollidableReference collidable);
        /// <summary>
        /// 检查是否对可碰撞目标的子级运行详细扫描测试。
        /// </summary>
        /// <param name="Collisible">可碰撞检查。</param>
        /// <param name="Child">要检查的冲突中的子项的索引。</param>
        /// <return>如果应尝试扫描测试,则为True,否则为False。</return>
        bool AllowTest(CollidableReference collidable, int child);
        /// <summary>
        /// 当扫描测试检测到T值非零的命中时调用。
        /// </summary>
        /// <param name="maxumT">对传递给遍历的maxumT的引用。</param>
        /// <param name="t">扫描测试的影响时间。</param>
        /// <param name="hitLocation">扫描检测到的第一个命中的位置。</param>
        /// <param name="hitNormal">命中位置的表面法线。</param>
        /// <param name="Collisible">遍历命中的冲突。</param>
        void OnHit(ref float maximumT, float t, in Vector3 hitLocation, in Vector3 hitNormal, CollidableReference collidable);
        /// <summary>
        /// 当扫描测试在T=0处检测到命中时调用,这意味着无法计算任何位置或法线。
        /// </summary>
        /// <param name="maxumT">对传递给遍历的maxumT的引用。</param>
        /// <param name="Collisible">遍历命中的冲突。</param>
        void OnHitAtZeroT(ref float maximumT, CollidableReference collidable);
    }

    partial class Simulation
    {
        // TODO：这对姿势精度都很敏感。如果你改变宽相位或姿态精度,这将不得不改变。

        struct ShapeRayHitHandler<TRayHitHandler> : IShapeRayHitHandler where TRayHitHandler : IRayHitHandler
        {
            public TRayHitHandler HitHandler;
            public CollidableReference Collidable;
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool AllowTest(int childIndex)
            {
                return HitHandler.AllowTest(Collidable, childIndex);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void OnRayHit(in RayData ray, ref float maximumT, float t, in Vector3 normal, int childIndex)
            {
                HitHandler.OnRayHit(ray, ref maximumT, t, normal, Collidable, childIndex);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe void GetPoseAndShape(CollidableReference reference, out RigidPose* pose, out TypedIndex shape)
        {
            if (reference.Mobility == CollidableMobility.Static)
            {
                var index = Statics.HandleToIndex[reference.StaticHandle.Value];
                pose = Statics.Poses.Memory + index;
                shape = Statics.Collidables[index].Shape;
            }
            else
            {
                ref var location = ref Bodies.HandleToLocation[reference.BodyHandle.Value];
                ref var set = ref Bodies.Sets[location.SetIndex];
                pose = set.Poses.Memory + location.Index;
                shape = set.Collidables[location.Index].Shape;
            }
        }
        struct RayHitDispatcher<TRayHitHandler> : IBroadPhaseRayTester where TRayHitHandler : IRayHitHandler
        {
            public Simulation Simulation;
            public ShapeRayHitHandler<TRayHitHandler> ShapeHitHandler;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public unsafe void RayTest(CollidableReference collidable, RayData* rayData, float* maximumT)
            {
                if (ShapeHitHandler.HitHandler.AllowTest(collidable))
                {
                    ShapeHitHandler.Collidable = collidable;
                    Simulation.GetPoseAndShape(collidable, out var pose, out var shape);
                    Simulation.Shapes[shape.Type].RayTest(shape.Index, *pose, *rayData, ref *maximumT, ref ShapeHitHandler);
                }
            }
        }

        /// <summary>
        /// 使光线与模拟相交。
        /// </summary>
        /// <typeparam name="THitHandler">要在光线-对象交叉点上执行的回调的类型。</typeparam>
        /// <param name="Origin">要投射的光线的原点。</param>
        /// <param name="direction">要投射的光线方向。</param>
        /// <param name="maxumT">射线遍历的最大长度(以方向长度为单位)。</param>
        /// <param name="hitHandler">要在光线-对象交叉点上执行的回调。</param>
        /// <param name="id">用户指定的射线ID。</param>
        public unsafe void RayCast<THitHandler>(in Vector3 origin, in Vector3 direction, float maximumT, ref THitHandler hitHandler, int id = 0) where THitHandler : IRayHitHandler
        {
            RayHitDispatcher<THitHandler> dispatcher;
            dispatcher.ShapeHitHandler.HitHandler = hitHandler;
            dispatcher.ShapeHitHandler.Collidable = default;
            dispatcher.Simulation = this;
            BroadPhase.RayCast(origin, direction, maximumT, ref dispatcher, id);
            // 命中处理程序被复制以将其传递给子处理;由于用户可能(并且很可能确实)依赖于突变,因此将其复制回原始引用。
            hitHandler = dispatcher.ShapeHitHandler.HitHandler;
        }

        unsafe struct SweepHitDispatcher<TSweepHitHandler> : IBroadPhaseSweepTester, ISweepFilter where TSweepHitHandler : ISweepHitHandler
        {
            public Simulation Simulation;
            public BufferPool Pool;
            public void* ShapeData;
            public int ShapeType;
            public RigidPose Pose;
            public BodyVelocity Velocity;
            public TSweepHitHandler HitHandler;
            public CollidableReference CollidableBeingTested;
            public float MinimumProgression;
            public float ConvergenceThreshold;
            public int MaximumIterationCount;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool AllowTest(int childA, int childB)
            {
                // 请注意,模拟扫描不允许非凸面扫描,因此我们不需要担心子A。
                return HitHandler.AllowTest(CollidableBeingTested, childB);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public unsafe void Test(CollidableReference reference, ref float maximumT)
            {
                if (HitHandler.AllowTest(reference))
                {
                    Simulation.GetPoseAndShape(reference, out var targetPose, out var shape);
                    Simulation.Shapes[shape.Type].GetShapeData(shape.Index, out var targetShapeData, out _);
                    // 请注意,出于模拟广角投射的目的,目标形状的速度被视为零。
                    // 如果要创建模拟速度感知扫描,则需要从为可碰撞参考设置的实体中拉出目标的速度
                    // 它们与非静力学有关。它看起来如下所示：
                    // 身体速度;目标速度;
                    // IF(reference ence.Mobility！=CollidableMobility.Static)
                    // {
                    // 参考变量位置=参考Simulation.Bodies.HandleToLocation[reference.Handle];
                    // //如果身体处于非活动状态,即使它们可以有很小的非零速度,您可能也应该将其视为零速度。
                    // //否则,您可能会得到一些不直观的结果,其中扫掠将非活动物体的速度向前积分,但模拟没有。
                    // IF(location.SetIndex==0)
                    // 目标速度=Simulation.Bodies.ActiveSet.Velocities[location.Index];
                    // 其他
                    // targetVelocity=new BodyVelocity();
                    // }
                    // 其他
                    // {
                    // targetVelocity=new BodyVelocity();
                    // }
                    CollidableBeingTested = reference;
                    var task = Simulation.NarrowPhase.SweepTaskRegistry.GetTask(ShapeType, shape.Type);
                    if (task != null)
                    {
                        var result = task.Sweep(
                            ShapeData, ShapeType, Pose.Orientation, Velocity,
                            targetShapeData, shape.Type, targetPose->Position - Pose.Position, targetPose->Orientation, new BodyVelocity(),
                            maximumT, MinimumProgression, ConvergenceThreshold, MaximumIterationCount,
                            ref this, Simulation.Shapes, Simulation.NarrowPhase.SweepTaskRegistry, Pool, out var t0, out var t1, out var hitLocation, out var hitNormal);
                        if (result)
                        {
                            if (t1 > 0)
                            {
                                hitLocation += Pose.Position;
                                HitHandler.OnHit(ref maximumT, t1, hitLocation, hitNormal, reference);
                            }
                            else
                            {
                                // t1==0时,hitLocation和hitNormal没有有效值,因此不要暗示它们存在。
                                HitHandler.OnHitAtZeroT(ref maximumT, reference);
                            }
                        }
                    }
                }
            }
        }


        /// <summary>
        /// 对照仿真扫掠形状。
        /// </summary>
        /// <typeparam name="TShape">要扫描的形状的类型。</typeparam>
        /// <typeparam name="TSweepHitHandler">扫描影响场景中的对象时执行的回调类型。</typeparam>
        /// <param name="Shape">要扫描的形状。</param>
        /// <param name="pose">扫描的开始姿势。</param>
        /// <param name="ocity">扫描形状的速度。</param>
        /// <param name="maxumT">用于积分速度的扫描的最大长度。</param>
        /// <param name="pool">执行期间要在其中分配任何临时资源的池。</param>
        /// <param name="hitHandler">扫掠影响场景中的对象时执行的回调。</param>
        /// <remarks>Simulation objects are treated as stationary during the sweep.</remarks>
        /// <param name="minumProgpression">以t参数表示的任何迭代扫描测试应为每个样本进行的最小进度量。</param>
        /// <param name="ConvergenceThreshold">阈值,表示允许迭代扫描测试在冲突中退出的t参数。</param>
        /// <param name="maxumIterationCount">迭代扫描测试中使用的最大迭代次数。</param>
        public unsafe void Sweep<TShape, TSweepHitHandler>(TShape shape, in RigidPose pose, in BodyVelocity velocity, float maximumT, BufferPool pool, ref TSweepHitHandler hitHandler,
            float minimumProgression, float convergenceThreshold, int maximumIterationCount)
            where TShape : unmanaged, IConvexShape where TSweepHitHandler : ISweepHitHandler
        {
            // 构建一个边界框。
            shape.ComputeAngularExpansionData(out var maximumRadius, out var maximumAngularExpansion);
            shape.ComputeBounds(pose.Orientation, out var min, out var max);
            BoundingBoxHelpers.GetAngularBoundsExpansion(velocity.Angular, maximumT, maximumRadius, maximumAngularExpansion, out var angularExpansion);
            min = min - angularExpansion + pose.Position;
            max = max + angularExpansion + pose.Position;
            var direction = velocity.Linear;
            SweepHitDispatcher<TSweepHitHandler> dispatcher;
            dispatcher.HitHandler = hitHandler;
            dispatcher.Pose = pose;
            dispatcher.Velocity = velocity;
            // 请注意,该形状是通过复制传递的,并且所有形状类型都要求是可快速复制的。没有GC洞。
            dispatcher.ShapeData = &shape;
            dispatcher.ShapeType = shape.TypeId;
            dispatcher.Simulation = this;
            dispatcher.Pool = pool;
            dispatcher.CollidableBeingTested = default;
            dispatcher.MinimumProgression = minimumProgression;
            dispatcher.ConvergenceThreshold = convergenceThreshold;
            dispatcher.MaximumIterationCount = maximumIterationCount;
            BroadPhase.Sweep(min, max, direction, maximumT, ref dispatcher);
            // 命中处理程序被复制以将其传递给子处理;由于用户可能(并且很可能确实)依赖于突变,因此将其复制回原始引用。
            hitHandler = dispatcher.HitHandler;
        }

        /// <summary>
        /// 对照仿真扫掠形状。
        /// </summary>
        /// <typeparam name="TShape">要扫描的形状的类型。</typeparam>
        /// <typeparam name="TSweepHitHandler">扫描影响场景中的对象时执行的回调类型。</typeparam>
        /// <param name="Shape">要扫描的形状。</param>
        /// <param name="pose">扫描的开始姿势。</param>
        /// <param name="ocity">扫描形状的速度。</param>
        /// <param name="maxumT">用于积分速度的扫描的最大长度。</param>
        /// <param name="pool">执行期间要在其中分配任何临时资源的池。</param>
        /// <param name="hitHandler">扫掠影响场景中的对象时执行的回调。</param>
        /// <remarks>Simulation objects are treated as stationary during the sweep.</remarks>
        public unsafe void Sweep<TShape, TSweepHitHandler>(in TShape shape, in RigidPose pose, in BodyVelocity velocity, float maximumT, BufferPool pool, ref TSweepHitHandler hitHandler)
            where TShape : unmanaged, IConvexShape where TSweepHitHandler : ISweepHitHandler
        {
            // 根据输入形状大小估计迭代扫描的合理终止条件。
            shape.ComputeAngularExpansionData(out var maximumRadius, out var maximumAngularExpansion);
            var minimumRadius = maximumRadius - maximumAngularExpansion;
            var sizeEstimate = Math.Max(minimumRadius, maximumRadius * 0.25f);
            // 默认情况下,倾向于精确度。这可能经常会触发最大迭代计数,但这没有关系。对性能敏感的用户可以在其他过载的情况下将其调低。
            // 如果新用户使用"快速"的默认调优,并得到明显不正确的结果,那将更加令人不安。
            var minimumProgressionDistance = .1f * sizeEstimate;
            var convergenceThresholdDistance = 1e-5f * sizeEstimate;
            var tangentVelocity = Math.Min(velocity.Angular.Length() * maximumRadius, maximumAngularExpansion / maximumT);
            var inverseVelocity = 1f / (velocity.Linear.Length() + tangentVelocity);
            var minimumProgressionT = minimumProgressionDistance * inverseVelocity;
            var convergenceThresholdT = convergenceThresholdDistance * inverseVelocity;
            var maximumIterationCount = 25;
            Sweep(shape, pose, velocity, maximumT, pool, ref hitHandler, minimumProgressionT, convergenceThresholdT, maximumIterationCount);
        }
    }
}
